#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import numpy as np
from ament_index_python.packages import get_package_share_directory

import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer, CancelResponse
from rclpy.action import ActionClient
from rcl_interfaces.msg import ParameterType
from action_msgs.msg import GoalStatus

# 导入ROS消息类型
from std_msgs.msg import Float32
from nav_msgs.msg import OccupancyGrid
from nav_msgs.msg import MapMetaData
from nav2_msgs.action import NavigateToPose


# NavigationClient类，处理导航客户端的功能
class NavigationClient(Node):
    def __init__(self):
        super().__init__('navigation_client')
        
        # 创建一个名为'navigate_to_pose'的动作客户端，使用NavigateToPose动作类型
        self._action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
        
        # 创建一个Explorer实例，用于访问占据栅格地图
        self.explorer = Explorer()
        
        self.send_goal()

    # goal_response_callback函数，处理导航目标的响应
    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info('Exploration goal rejected')
            return

        self.get_logger().info('Navigation goal accepted')

        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    # get_result_callback函数，处理导航目标完成后的结果
    def get_result_callback(self, future):
        result = future.result().result
        status = future.result().status
        if status == GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().info('Arrived at destination')
            return 
        else:
            self.get_logger().info('Goal failed with status: {0}'.format(status))
            return 

        # rclpy.spin_once(self.explorer)

    def feedback_callback(self, feedback_msg):
        pass
        # self.get_logger().info('Received feedback: {0}'.format(feedback_msg))

    # send_goal函数，发送导航目标请求
    def send_goal(self):
        self.get_logger().info('Waiting for action server...')
        self._action_client.wait_for_server()
        # waypoint = [1.42,0.89]
        rclpy.spin_once(self.explorer)  # 刷新可访问的路径点
        waypoint = self.explorer.sorted_accessible_waypoints[0]  # 获取第一个路径点
        self.explorer.sorted_accessible_waypoints = self.explorer.sorted_accessible_waypoints[1:]  # 弹出第一个元素

        # 构造导航目标消息
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose.header.frame_id = 'map'
        goal_msg.pose.pose.position.x = float(waypoint[0])
        goal_msg.pose.pose.position.y = float(waypoint[1])

        self.get_logger().info(
            'Sending navigation goal request x: ' + str(round(goal_msg.pose.pose.position.x, 2)) + ' y: ' + str(
                round(goal_msg.pose.pose.position.y, 2)))

        # 发送导航目标请求并设置回调函数
        self._send_goal_future = self._action_client.send_goal_async(goal_msg, self.feedback_callback)
        self._send_goal_future.add_done_callback(self.goal_response_callback)

        rclpy.spin_until_future_complete(self, self._send_goal_future)

        goal_handle = self._send_goal_future.result()
        get_result_future = goal_handle.get_result_async()

        rclpy.spin_until_future_complete(self, get_result_future)


class Explorer(Node):
    def __init__(self):
        super().__init__('explorer')
        
        # 创建一个订阅器，订阅'map'话题，消息类型为OccupancyGrid，回调函数为occupancy_callback
        self.occupancy_subscription = self.create_subscription(OccupancyGrid, 'map', self.occupancy_callback, 10)

        # 生成一系列路径点
        self.waypoints = self.generate_list_of_waypoints(n_of_waypoints=100, step=0.2)
        
        # 初始化可访问的路径点、排序后的可访问路径点和占据概率值
        self.accessible_waypoints = np.array([])
        self.sorted_accessible_waypoints = np.array([])
        self.occupancy_value = np.array([])

    # occupancy_callback函数，处理占据栅格地图数据的回调
    def occupancy_callback(self, msg):
        data = np.array(msg.data)  # 获取占据栅格地图数据
        current_map_width = msg.info.width  # 获取当前地图宽度
        current_map_height = msg.info.height  # 获取当前地图高度
        resolution = msg.info.resolution  # 获取地图分辨率

        # 将数据reshape成地图的形状
        data = np.reshape(data, (current_map_height, current_map_width))

        # 遍历每个路径点，保存可访问的路径点
        self.accessible_waypoints = np.array([])
        self.occupancy_value = np.array([])
        for waypoint in self.waypoints:
            try:
                occupancy_grid_coordinates = [int((waypoint[1] + 2.3) / resolution), int((waypoint[0] + 2.3) / resolution)]
                conv = self.convolute(data, occupancy_grid_coordinates, size=9)  # 执行卷积操作

                # 如果卷积返回True，表示路径点可访问，保存在self.accessible_waypoints中
                if conv[0]:
                    self.accessible_waypoints = np.append(self.accessible_waypoints, waypoint)
                    self.occupancy_value = np.append(self.occupancy_value, conv[1])
            except IndexError:
                pass

        # 将可访问路径点reshape成(n, 2)形状
        self.accessible_waypoints = self.accessible_waypoints.reshape((-1, 2))

        # 根据占据概率值对路径点进行排序，以便机器人优先访问不确定性较高的路径点（发现边界）
        occupancy_value_idxs = self.occupancy_value.argsort()
        self.sorted_accessible_waypoints = self.accessible_waypoints[occupancy_value_idxs[::-1]]

        # 如果排序后的可访问路径点为空，添加一些硬编码的路径点，以便开始导航
        if np.size(self.sorted_accessible_waypoints) == 0:
            self.sorted_accessible_waypoints = np.array([[1.5, 0.0], [0.0, 1.5], [-1.5, 0.0], [0.0, -1.5]])

        # 将新的路径点保存在self.sorted_accessible_waypoints中，以供导航客户端使用
        self.get_logger().info('Accessible waypoints have been updated...')

    @staticmethod
    def convolute(data, coordinates, size=3, threshold=40):
        """
        计算以'coordinates'为中心，大小为(size x size)的区域内的平均占据概率。

        :param data: 占据栅格地图数据（形状为(x, y)的地图维度）
        :param coordinates: 占据栅格地图坐标
        :param size: 卷积核大小
        :param threshold: 可访问性阈值
        :return: 如果路径点可访问返回True，否则返回False，以及卷积后的平均占据概率
        """
        sum = 0
        for x in range(int(coordinates[0] - size / 2), int(coordinates[0] + size / 2)):
            for y in range(int(coordinates[1] - size / 2), int(coordinates[1] + size / 2)):
                if data[x, y] == -1:
                    sum += 100
                elif data[x, y] > 50:
                    sum += 1000000
                else:
                    sum += int(data[x, y])

        average = sum / (size * size)
        if average < threshold:
            return True, average
        else:
            return False, average

    def generate_list_of_waypoints(self, n_of_waypoints, step):
        """
        生成一个大小为（'n_of_waypoints' * 'n_of_waypoints'）的路径点网格，步长为'step'

        :param n_of_waypoints: 每一边生成的路径点数量
        :param step: 路径点间的分辨率
        :return waypoints: 2D numpy数组，存储路径点坐标
        """
        waypoints = np.zeros((n_of_waypoints * n_of_waypoints, 2))

        i = 0
        for index_y in range(n_of_waypoints):
            for index_x in range(n_of_waypoints):
                waypoints[i] = [float(index_x) / (1 / step), float(index_y) / (1 / step)]
                i += 1

        self.get_logger().info("Grid of waypoints has been generated.")
        return waypoints
    
def main(args=None):
    rclpy.init(args=args)

    navigation_client = NavigationClient()
    rclpy.spin(navigation_client)

    rclpy.shutdown()


if __name__ == '__main__':
    main()    